#include "motor_init.h"
#include <linux/can.h>
#include <string.h>
#include <stdio.h>
#include <list>
#include <iostream>
#include <unistd.h>
#include <pthread.h>

#include <sys/ipc.h>
#include <sys/types.h>
#include <sys/msg.h>

#include "msg_def.h"

#include "procedure.h"
#include "config.h"
#include "common.h"
#include "ack_msg.h"

int get_retback_pos(int sidIn)
{
    int pos_slow = 500;
    if(getELeCtlMaxD(sidIn) <= pos_slow)
    {
       pos_slow =  (getELeCtlMaxD(sidIn)<<2);
    }

    if(sidIn == SID_DP1_X_INIT)
    {
        pos_slow = 500;
    }
    else if(sidIn == SID_DJP_PUMP_SMALL_INIT
    ||sidIn == SID_DJP_PUMP_BIG_INIT
    ||sidIn == SID_DJQ_PUMP_INIT
    ||sidIn == SID_DP8_1_PUMP_INIT
    ||sidIn == SID_DJQ_Z_INIT
    ||SID_DP8_1_Z_INIT == sidIn)
    {
        pos_slow = 1500;
    }
    else if(SID_DP8_DROPTIP_INIT == sidIn)
    {
        pos_slow = 2500;
    }

    return pos_slow;
}

int get_retback_speed(int sidIn)
{
    int speed_slow = 1000;
    return speed_slow;
}


int get_retback_acc(int sidIn)
{    
    int acc_slow = 4000;
    return acc_slow;
}


int get_init_high_speed(int sidIn)
{
   int high_speed = 30000;
   if(sidIn==SID_DP1_X_INIT || SID_DP8_X_INIT == sidIn
   || SID_DP8_DROPTIP_INIT == sidIn)
   {
       high_speed = 6000;
   }
   else if(sidIn==SID_DP1_Y_INIT || SID_DP8_Y_INIT == sidIn
   || SID_DJP_Y_INIT == sidIn
   || SID_DJP_X_INIT == sidIn 
   || SID_DJQ_X_INIT == sidIn
   || SID_DJQ_Y_INIT == sidIn
   || SID_QPCR1_Y == sidIn
   || SID_QPCR2_Y == sidIn )
   {
       high_speed = 5000;
   }
   else if(SID_DJQ_JAW_INIT == sidIn || sidIn == SID_DP8_JAW_INIT)
   {
       high_speed = 4000;       
   }
   else if(SID_LOG_P_X_CH == sidIn)
   {
#ifdef _QPCR
       high_speed = 8000;//for log3 jaw x
#else
       high_speed = 20000; // for log1 x tray      
#endif
   }
   else if(sidIn==SID_DP8_1_Z_INIT)
   {
       high_speed = 10000;
   }
   
   return high_speed;
}

int get_init_high_acc(int sidIn)
{
   int high_acc = 80000;
   if(sidIn==SID_DP1_X_INIT || SID_DP8_X_INIT == sidIn
      || sidIn==SID_DP8_1_Z_INIT)
   {
       high_acc = 8000;
   }
   else if(SID_LOG_P_X_CH == sidIn)
   {
       //high_acc = 20000;
#ifdef _QPCR
       high_acc = 8000;//for log3 jaw x
#else
       high_acc = 40000; // for log1 x tray      
#endif

   }
   return high_acc;
}

//using the function inner the uim controller
void CUim_MainCtl::uim_motor_init(int sidIn)
{
  uim_data dataRet;
  struct can_frame frameToSend;
  uim_frame dataRead;

  int count = 0;
  //move_param * stMoveParam = (move_param *)moveParamIn;
  unsigned char sid = sidIn;
   move_param moveParam;

    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


  #define MAX_WAIT_CHECKING_COUNT 80

    setELeCtlStatus(sid,CHECKING_RET_INIT);
  //1, get current switch status
    format_cmd(1,SFB,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);    

    int comValue = MAX_WAIT_CHECKING_COUNT/3;
    count = 0;
    while(getELeCtlStatus(sid) == CHECKING_RET_INIT)
    {
        count++;
        usleep(50000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }

        if(count%comValue==0)
        {
            printf("send SFB\n");
            this->WriteCanPort(&frameToSend);
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("sid[%d],check ret back timeout,status[%d]\n",sid,getELeCtlStatus(sid));

        sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);

        return;
    }
    else if(getELeCtlStatus(sid) == ALREADY_ON_ORIGIN)
    {
        printf("sid[%d],already on origin point\n",sid);
        // go away origin point a short distance
        move_param moveParam;
        moveParam.dir = 1-getELeCtlRetdir(sid);
        moveParam.dec_speed = get_retback_acc(sid);
        moveParam.inc_speed = moveParam.dec_speed;
        moveParam.pos = get_retback_pos(sid);
        moveParam.speed = get_retback_speed(sid);
        
        setELeCtlStatus(sid,INIT_AWAY_ORIGING);

        this->uim_move_relative(&moveParam);

        count = 0;
        while(getELeCtlStatus(sid) == INIT_AWAY_ORIGING)
        {
            count++;
            usleep(200000);
            if(count>MAX_WAIT_CHECKING_COUNT)
            {
                break;
            }

            //printf("in loop,count:%d\n",count);
        }

        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            printf("sid[%d],move away origin timeout,status[%d]\n",sid,getELeCtlStatus(sid));

            sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);

            return;
        }

    }
    else
    {
        //move to origin fast and then move away origin
        //setSensorMoveParam(sid);

        
        moveParam.dec_speed = get_init_high_acc(sid);
        moveParam.speed = get_init_high_speed(sid);
        moveParam.inc_speed = moveParam.dec_speed;
        
        //1, set the sample mode
        unsigned char interval[4] = {0x00,0x00,0x00};
        format_cmd(1,STG,sid,3, interval,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //4,STOn binding act to S1IE
        //printf("act ret back now!,sid[%d],222\n",sid);
        //5.1 set the incease speed
        int inc_speed = moveParam.inc_speed;
        printf("to origin 1, set inc_speed to %d\n",inc_speed);
        unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
                    (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
        };
        
        format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //5.2 set the decrease speed
        int dec_speed = moveParam.dec_speed;
        printf("to origin 1,set dec_speed to %d\n",dec_speed);
        unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
                    (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
        };
        
        format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
        //printf("act ret back now!,sid[%d],333\n",sid);

        int tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
        
        //printf("ret back, set pos to %d\n",tmpPos);
        unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
        dataPos[0] = tmpPos&0x00ff;
        dataPos[1] = (tmpPos>>8)&0x00ff;
        dataPos[2] = (tmpPos>>16)&0x00ff;
        dataPos[3] = (tmpPos>>24)&0x00ff;
        //set position to 0, change to speed mode
        format_cmd(1,STP,sid,4, dataPos,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //5,set the speed
        int speed = moveParam.speed;
        if(getELeCtlRetdir(sid) == 0)
        {
            speed = 0 - speed;
        }
        printf("to origin 1, ret back, set speed to %d\n",speed);
        unsigned char speedCode[4] = {speed&0x00ff,
                    (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
        format_cmd(1,SPD,sid,4, speedCode,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        setELeCtlStatus(sid,INIT_TO_ORIG_1);
        //wait here for 
        count = 0;
        while(getELeCtlStatus(sid) == INIT_TO_ORIG_1)
        {
            count++;
            usleep(500000);
            if(count>MAX_WAIT_CHECKING_COUNT)
            {
                break;
            }
        }

        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            printf("sid[%d],to origin 1 timeout,status[%d]\n",sid,getELeCtlStatus(sid));

            sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);

            return;
        }
        

    }
    //here, the motor is away origin point a short distance
    printf(" the motor of sid[%d] is short distance away origin now\n");

    // setSensorAction(sid,1);//stop when sensor up or down

        
    moveParam.dec_speed = get_retback_acc(sid);
    moveParam.speed = get_retback_speed(sid);
    moveParam.inc_speed = moveParam.dec_speed;
    
    //1, set the sample mode
    unsigned char interval[4] = {0x00,0x00,0x00};
    format_cmd(1,STG,sid,3, interval,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //4,STOn binding act to S1IE
    //printf("act ret back now!,sid[%d],222\n",sid);
    //5.1 set the incease speed
    int inc_speed = moveParam.inc_speed;
    printf("ret back, set inc_speed to %d\n",inc_speed);
    unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
                (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
    };
    
    format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //5.2 set the decrease speed
    int dec_speed = moveParam.dec_speed;
    printf("ret back, set dec_speed to %d\n",dec_speed);
    unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
                (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
    };
    
    format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //printf("act ret back now!,sid[%d],333\n",sid);

    int tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
    
    //printf("ret back, set pos to %d\n",tmpPos);
    unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
    dataPos[0] = tmpPos&0x00ff;
    dataPos[1] = (tmpPos>>8)&0x00ff;
    dataPos[2] = (tmpPos>>16)&0x00ff;
    dataPos[3] = (tmpPos>>24)&0x00ff;

    format_cmd(1,STP,sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //5,set the speed
    int speed = moveParam.speed;
    if(getELeCtlRetdir(sid) == 0)
    {
        speed = 0 - speed;
    }
    printf("ret back, set speed to %d\n",speed);
    unsigned char speedCode[4] = {speed&0x00ff,
                (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    format_cmd(1,SPD,sid,4, speedCode,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);   
      
}

//using the steps self designed
void CUim_MainCtl::uim_motor_init1(int sidIn)
{
  uim_data dataRet;
  struct can_frame frameToSend;
  uim_frame dataRead;

  int count = 0;
  //move_param * stMoveParam = (move_param *)moveParamIn;
  unsigned char sid = sidIn;
   move_param moveParam;

// #ifdef _QPCR
//    if(sid == 45 || sid == 46
//     ||sid == 52 || sid == 53 )
//    {
//         moveParam.dec_speed = get_retback_acc(sid);
//         moveParam.speed = get_retback_speed(sid);
//         moveParam.inc_speed = moveParam.dec_speed;
//         moveParam.sid = sid;
//         uim_ret_back(&moveParam);

//         return;
//    }
// #endif


    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


  #define MAX_WAIT_CHECKING_COUNT 80

    setELeCtlStatus(sid,CHECKING_RET_INIT);
  //1, get current switch status
    format_cmd(1,SFB,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);    

    int comValue = MAX_WAIT_CHECKING_COUNT/3;
    count = 0;
    while(getELeCtlStatus(sid) == CHECKING_RET_INIT)
    {
        count++;
        usleep(50000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }

        if(count%comValue==0)
        {
            printf("send SFB\n");
            this->WriteCanPort(&frameToSend);
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("sid[%d],check ret back timeout,status[%d]\n",sid,getELeCtlStatus(sid));

        
        if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
        {
           sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);
        }
        else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
           recvDataEleCtl stDataRecv;

           memset(&stDataRecv,0,sizeof(recvDataEleCtl));
           stDataRecv.sid = sid;
           stDataRecv.frameType = CHECK_ACT_TIMEOUT;
           sendAckToLua(&stDataRecv);
        }

        return;
    }
    else if(getELeCtlStatus(sid) == ALREADY_ON_ORIGIN)
    {
        printf("sid[%d],already on origin point\n",sid);        

    }
    else
    {
       
        moveParam.dec_speed = get_init_high_acc(sid);
        moveParam.speed = get_init_high_speed(sid);
        moveParam.inc_speed = moveParam.dec_speed;

        setSensorAction(sid,1);//stop when sensor up or down
        
        //1, set the sample mode
        unsigned char interval[4] = {0x00,0x00,0x00};
        format_cmd(1,STG,sid,3, interval,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //4,STOn binding act to S1IE
        //printf("act ret back now!,sid[%d],222\n",sid);
        //5.1 set the incease speed
        int inc_speed = moveParam.inc_speed;
        printf("to origin 1, set inc_speed to %d\n",inc_speed);
        unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
                    (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
        };
        
        format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //5.2 set the decrease speed
        int dec_speed = moveParam.dec_speed;
        printf("to origin 1,set dec_speed to %d\n",dec_speed);
        unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
                    (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
        };
        
        format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
        //printf("act ret back now!,sid[%d],333\n",sid);

        int tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
        
        //printf("ret back, set pos to %d\n",tmpPos);
        unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
        dataPos[0] = tmpPos&0x00ff;
        dataPos[1] = (tmpPos>>8)&0x00ff;
        dataPos[2] = (tmpPos>>16)&0x00ff;
        dataPos[3] = (tmpPos>>24)&0x00ff;
        //set position to 0, change to speed mode
        format_cmd(1,STP,sid,4, dataPos,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        //5,set the speed
        int speed = moveParam.speed;
        if(getELeCtlRetdir(sid) == 0)
        {
            speed = 0 - speed;
        }
        printf("to origin 1, set speed to %d\n",speed);
        unsigned char speedCode[4] = {speed&0x00ff,
                    (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
        format_cmd(1,SPD,sid,4, speedCode,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);

        setELeCtlStatus(sid,INIT_TO_ORIG_1);
        //wait here for 
        count = 0;
        while(getELeCtlStatus(sid) == INIT_TO_ORIG_1)
        {
            count++;
            usleep(200000);
            if(count>MAX_WAIT_CHECKING_COUNT)
            {
                break;
            }

            format_cmd(1,SFB,sid,0, NULL,&dataRet);
            encap_uim_data(&dataRet,&frameToSend);     
            this->WriteCanPort(&frameToSend);
        }

        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            printf("sid[%d],to origin 1 timeout,status[%d]\n",sid,getELeCtlStatus(sid));

            sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);

            return;
        }
        

    }
    //here, the motor is now on origin
    printf("achieved origin point first time\n");
    //sleep(1);
    // go away origin point a short distance
    memset(&moveParam,0,sizeof(move_param));
    moveParam.dir = 1-getELeCtlRetdir(sid);
    moveParam.dec_speed = get_retback_acc(sid);
    moveParam.inc_speed = moveParam.dec_speed;
    moveParam.pos = get_retback_pos(sid);
    moveParam.speed = get_retback_speed(sid);

    setSensorAction(sid,0);
    
    

    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    unsigned char dataPos[4] = {0,0,0,0};
     format_cmd(1,STP,sid,4, dataPos,&dataRet);
     encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    
    //usleep(2000);
    //4, set the speed
    unsigned int speed = moveParam.speed;
    printf("away origin point, set speed to %d\n",speed);
    unsigned char dataSpeed[4] = {speed&0x00ff,
    (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    format_cmd(1,SPD,sid,4, dataSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);



     //4.1, set the increase speed
    int inc_speed = moveParam.inc_speed;
    printf("away origin point, set inc_speed to %d\n",inc_speed);
    unsigned char dataIncSpeed[4] = {inc_speed&0x00ff,
    (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff};
    format_cmd(1,MAC,sid,4, dataIncSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //4.2 set the decrease speed
    unsigned int dec_speed = moveParam.dec_speed;
    printf("away origin point, set dec_speed to %d\n",dec_speed);
    unsigned char dataDecSpeed[4] = {dec_speed&0x00ff,
    (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff};
    format_cmd(1,MDE,sid,4, dataDecSpeed,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);


    //5,set the position desired
    setELeCtlStatus(sid,INIT_AWAY_ORIGING);
    int tmpPos = (int)moveParam.pos;
    if(moveParam.dir == 0)
    {
        tmpPos = 0 - tmpPos;
    }

    //unsigned char dataPos[4] = {tmpPos&0x00ff,
    //(tmpPos>>8)&0x00ff,(tmpPos>>16)&0x00ff,(tmpPos>>24)&0x00ff};
    dataPos[0] = tmpPos&0x00ff;
    dataPos[1] = (tmpPos>>8)&0x00ff;
    dataPos[2] = (tmpPos>>16)&0x00ff;
    dataPos[3] = (tmpPos>>24)&0x00ff;
    printf("away origin point, set pos to %d\n",tmpPos);
    unsigned char tmpPosCmd = STP;
    format_cmd(1,tmpPosCmd,sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);    
    this->WriteCanPort(&frameToSend);

    

    count = 0;
    while(getELeCtlStatus(sid) == INIT_AWAY_ORIGING)
    {
        count++;
        usleep(200000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("sid[%d],move away origin timeout,status[%d]\n",sid,getELeCtlStatus(sid));

        sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);

        return;
    }

    printf(" the motor of sid[%d] is short distance away origin now\n");
    //check if has left light switch
    // setELeCtlStatus(sid,GETTING_SWITH_STATUS);
    // int currSwitch = 1;
    // setELeCtlBackData(sid,&currSwitch);
    // format_cmd(1,SFB,sid,0, NULL,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    
    // count = 0;
    // while(getELeCtlStatus(sid) == GETTING_SWITH_STATUS)
    // {
    //     count++;
    //     usleep(200000);
    //     if(count>(MAX_WAIT_CHECKING_COUNT>>3))
    //     {
    //         break;
    //     }
    //     //format_cmd(1,SFB,sid,0, NULL,&dataRet);
    //     //encap_uim_data(&dataRet,&frameToSend);     
    //     this->WriteCanPort(&frameToSend);
    //     //printf("in loop,count:%d\n",count);
    // }

    // if(count>(MAX_WAIT_CHECKING_COUNT>>3))
    // {
    //     printf("sid[%d],check ,status[%d]\n",sid,getELeCtlStatus(sid));
    //     if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
    //     {
    //        sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);
    //     }
    //     else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
    //     {
    //        recvDataEleCtl stDataRecv;

    //        memset(&stDataRecv,0,sizeof(recvDataEleCtl));
    //        stDataRecv.sid = sid;
    //        stDataRecv.frameType = CHECK_ACT_TIMEOUT;
    //        sendAckToLua(&stDataRecv);
    //     }
        
    //     return;
    // }
    
    // if(currSwitch == getELeCtlRetflag(sid))
    // {    //on origin point also
    //      if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
    //     {
    //        sendAckToGrpc(0,0,sid,-1);
    //     }
    //     else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
    //     {
    //        recvDataEleCtl stDataRecv;

    //        memset(&stDataRecv,0,sizeof(recvDataEleCtl));
    //        stDataRecv.sid = sid;
    //        stDataRecv.frameType = CHECK_RET_BACK_END;
    //        sendAckToLua(&stDataRecv);
    //     }
    //     printf("move away origin point failed, ret back end now\n");
    //     setELeCtlBackData(sid,NULL);
    //     return ;
    // }
    
    //here get status success;
    setSensorAction(sid,1);//stop when sensor up or down

        
    moveParam.dec_speed = get_retback_acc(sid);
    moveParam.speed = get_retback_speed(sid);
    moveParam.inc_speed = moveParam.dec_speed;
    moveParam.sid = sid;
    uim_init_last_step(&moveParam);
    
//    //1, set the sample mode
//    unsigned char interval[4] = {0x00,0x00,0x00};
//    format_cmd(1,STG,sid,3, interval,&dataRet);
//    encap_uim_data(&dataRet,&frameToSend);     
//    this->WriteCanPort(&frameToSend);
//
//    //4,STOn binding act to S1IE
//    //printf("act ret back now!,sid[%d],222\n",sid);
//    //5.1 set the incease speed
//    inc_speed = moveParam.inc_speed;
//    printf("ret back, set inc_speed to %d\n",inc_speed);
//    unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
//                (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
//    };
//    
//    format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
//    encap_uim_data(&dataRet,&frameToSend);     
//    this->WriteCanPort(&frameToSend);
//
//    //5.2 set the decrease speed
//    dec_speed = moveParam.dec_speed;
//    printf("ret back, set dec_speed to %d\n",dec_speed);
//    unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
//                (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
//    };
//    
//    format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
//    encap_uim_data(&dataRet,&frameToSend);     
//    this->WriteCanPort(&frameToSend);
//    //printf("act ret back now!,sid[%d],333\n",sid);
//
//    tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
//    
//    //printf("ret back, set pos to %d\n",tmpPos);
//    //unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
//    dataPos[0] = tmpPos&0x00ff;
//    dataPos[1] = (tmpPos>>8)&0x00ff;
//    dataPos[2] = (tmpPos>>16)&0x00ff;
//    dataPos[3] = (tmpPos>>24)&0x00ff;
//    //set inc pos to 0, change to speed mode
//    format_cmd(1,STP,sid,4, dataPos,&dataRet);
//    encap_uim_data(&dataRet,&frameToSend);     
//    this->WriteCanPort(&frameToSend);
//    
//    setELeCtlStatus(sid, RET_BACKING);
//    //5,set the speed
//    speed = moveParam.speed;
//    if(getELeCtlRetdir(sid) == 0)
//    {
//        speed = 0 - speed;
//    }
//    printf("ret back, set speed to %d\n",speed);
//    unsigned char speedCode[4] = {speed&0x00ff,
//                (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
//    format_cmd(1,SPD,sid,4, speedCode,&dataRet);
//    encap_uim_data(&dataRet,&frameToSend);     
//    this->WriteCanPort(&frameToSend);   
      
}

void CUim_MainCtl::uim_init_last_step(void * moveParamIn)
{
  //1,set mcfg, enable S1IE, MCF
  uim_data dataRet;
  struct can_frame frameToSend;
  uim_frame dataRead;

  int count = 0;
  move_param * stMoveParam = (move_param *)moveParamIn;
  unsigned char sid = stMoveParam->sid;

  #define MAX_WAIT_CHECKING_COUNT 80

//   unsigned char cmdCode = POS;
//   if(uimCtlArr[sid-10].loopType == CLOSE_LOOP)
//   {
//       cmdCode = QEC;
//   }

    format_cmd(1,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    setELeCtlStatus(sid,CHECKING_RET_INIT);
  //1, get current switch status
    format_cmd(1,SFB,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //uimCtlArr[sid-10].status = CHECKING_RET;

    

    int comValue = MAX_WAIT_CHECKING_COUNT/3;
    count = 0;
    while(getELeCtlStatus(sid) == CHECKING_RET_INIT)
    {
        count++;
        usleep(50000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }

        if(count%comValue==0)
        {
            printf("send SFB\n");
            this->WriteCanPort(&frameToSend);
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("sid[%d],check ret back timeout,status[%d]\n",sid,getELeCtlStatus(sid));
        if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
        {
           sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);
        }
        else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
           recvDataEleCtl stDataRecv;

           memset(&stDataRecv,0,sizeof(recvDataEleCtl));
           stDataRecv.sid = sid;
           stDataRecv.frameType = CHECK_ACT_TIMEOUT;
           sendAckToLua(&stDataRecv);
        }       

        return;
    }
    else if(getELeCtlStatus(sid) == ALREADY_ON_ORIGIN)
    {
        printf("sid[%d],already on origin point\n",sid);
        //sendAckToGrpc(0,0,sid,-1);
        setELeCtlCurrPos(sid,0);
        if(getELeCtlAckFlag(sid) == MSG_FROM_GRPC)
        {
           sendAckToGrpc(0,0,sid,-1);
        }
        else if(getELeCtlAckFlag(sid) == MSG_FROM_LUA)
        {
           recvDataEleCtl stDataRecv;

           memset(&stDataRecv,0,sizeof(recvDataEleCtl));
           stDataRecv.sid = sid;
           stDataRecv.frameType = CHECK_RET_BACK_END;
           sendAckToLua(&stDataRecv);
        }
        
        return;
    }

     //printf("act ret back now!,sid[%d],111\n",sid);

  //format_cmd(1,MCF,sid,0, NULL,&dataRet);
  //encap_uim_data(&dataRet,&frameToSend);     
  //this->WriteCanPort(&frameToSend);



  //2,set S1FACT 0x04; SCF
//   unsigned char s1con[4] = {0xc0,0x00,0x00};// stop and set the pos to 0

//   if(getELeCtlRetflag(sid) == STOP_WHEN_UP)
//   {
//       if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
//       {
//           s1con[0] = 0xb0; //decrease the speed to zero 
//       }
//       if(sid==51)
//       {
//           s1con[1] = 0xc0; 
//       }
      
//   }
//   else if(getELeCtlRetflag(sid) == STOP_WHEN_DOWN)
//   {
//       s1con[0] = 0x0c; 
//       if( sid==17 )//|| sid==52 || sid==53)sid==45 || sid==46 ||
//       {
//             s1con[0] = 0x0B; //decrease the speed to zero 
//       }
//       if(sid==51)
//       {
//             s1con[1] = 0x0C; 
//       }
//   }
  
//   format_cmd(1,SCF,sid,3, s1con,&dataRet);
//   encap_uim_data(&dataRet,&frameToSend);     
//   this->WriteCanPort(&frameToSend);
     setSensorAction(sid,1);

  //3, set sample module
  unsigned char interval[4] = {0x00,0x00,0x00};
  format_cmd(1,STG,sid,3, interval,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //4,STOn binding act to S1IE
//printf("act ret back now!,sid[%d],222\n",sid);
   //5.1 set the incease speed
   int inc_speed = stMoveParam->inc_speed;
   printf("ret back, set inc_speed to %d\n",inc_speed);
   unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
            (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
   };
  
  format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

  //5.2 set the decrease speed
  int dec_speed = stMoveParam->dec_speed;
  printf("ret back, set dec_speed to %d\n",dec_speed);
   unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
            (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
   };
  
  format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);
  //printf("act ret back now!,sid[%d],333\n",sid);

    int tmpPos = 0;//(int)uimCtlArr[sid-10].max_distance;
    
     if(getELeCtlRetdir(sid)==0)
     {
         tmpPos = 0 - tmpPos;
     }
    //tmpPos = -20000;
    //printf("ret back, set pos to %d\n",tmpPos);
    unsigned char dataPos[4] = {0x00,0x00,0x00,0x00}; 
     dataPos[0] = tmpPos&0x00ff;
     dataPos[1] = (tmpPos>>8)&0x00ff;
     dataPos[2] = (tmpPos>>16)&0x00ff;
     dataPos[3] = (tmpPos>>24)&0x00ff;

    format_cmd(1,STP,sid,4, dataPos,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

  //5,set the speed
  int speed = stMoveParam->speed;
  if(getELeCtlRetdir(sid) == 0)
  {
      speed = 0 - speed;
  }
  printf("ret back, set speed to %d\n",speed);
  unsigned char speedCode[4] = {speed&0x00ff,
            (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
  format_cmd(1,SPD,sid,4, speedCode,&dataRet);
  encap_uim_data(&dataRet,&frameToSend);     
  this->WriteCanPort(&frameToSend);

    

    printf("act ret back now!,sid[%d]\n",sid);
    //uimCtlArr[sid-10].status = RET_BACKING;
    setELeCtlStatus(sid,RET_BACKING);
    int currPos = getELeCtlCurrPos(sid);
    currPos = (currPos>0)?currPos:(0-currPos);

    
    int maxCnt = MAX_WAIT_CHECKING_COUNT;
    if(stMoveParam->speed && currPos)
    {
        maxCnt = (currPos*10)/(stMoveParam->speed);
    }
    count = 0;
    while(getELeCtlStatus(sid) == RET_BACKING)
    {
        count++;
        usleep(100000);
        if(count>maxCnt)
        {
            break;
        }
        //printf("in loop,count:%d\n",count);
    }

    count = 0;
    while(getELeCtlStatus(sid) == RET_BACKING)
    {
        count++;
        usleep(200000);
        if(count>MAX_WAIT_CHECKING_COUNT)
        {
            break;
        }
        
        format_cmd(1,SFB,sid,0, NULL,&dataRet);
        encap_uim_data(&dataRet,&frameToSend);     
        this->WriteCanPort(&frameToSend);
        //printf("in loop,count:%d\n",count);
    }

    if(count>MAX_WAIT_CHECKING_COUNT)
    {
        printf("wait ret back timeout\n");       
        return;
    }
    else
    {
        sendAckToGrpc(0,0,sid,-1);
    }
}

void CUim_MainCtl::setSensorMoveParam(int sidIn)
{
    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;
    
    unsigned char sid = sidIn;
    // format_cmd(1,ENA,sid,0, NULL,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    //set off the controller
    printf("1, set off the controller\n");
    format_cmd(0,OFF,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //1, set the speed

    sleep(1);
    printf("2, set the speed\n");
    int speed = get_retback_speed(sid);

    unsigned char dataBuff[4] = {speed&0x00ff,(speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};

    format_cmd(0,SPD,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    
    //2,set the position
    sleep(1);
    printf("3, set the position\n");
    int pos = get_retback_pos(sid);

    memcpy(dataBuff,&pos,4);
    
    format_cmd(0,STP,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    
    //3,set the acc
    sleep(1);
    printf("4, set the acc\n");
    int acc = get_retback_acc(sid);
    memcpy(dataBuff,&acc,4);
    
    format_cmd(0,MAC,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    
    //4,set the dec
    sleep(1);
    printf("5, set the dec\n");
    format_cmd(0,MDE,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    
    
    int retFlag = getELeCtlRetflag(sid);

    unsigned char s1con[4] = {0x90,0x00,0x00};

    if(retFlag == STOP_WHEN_DOWN)
    {
        dataBuff[0] = 3;
        s1con[0] = 0x09;

    }
    else
    {
        dataBuff[0] = 2;
        s1con[0] = 0x90;
    }
    //5, bind the move param above to sensor up or down edge
    usleep(100);
    printf("6, set the sto\n");
    format_cmd(0,STO,sid,1, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    printf("in setSensorMoveParam, set SCF, s1con:0x%02x,0x%02x,0x%02x\n",
    s1con[0]&0x00ff,s1con[1]&0x00ff,s1con[2]&0x00ff);
    //6,set the motor to move revert when sensor up or down edge
    usleep(100);
    printf("7, set the scf\n");
    format_cmd(0,SCF,sid,3, s1con,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    
    //set the speed to 0, to avoid the motor run after enable
    memset(dataBuff,0,4);
    usleep(100);
    printf("8, set the speed to 0\n");
    format_cmd(0,SPD,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    
    //enable the uim controller
    usleep(100);
    printf("9, enable the controller\n");
    format_cmd(0,ENA,sid,0, NULL,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    //set postion to 0, change to speed trace mode
    usleep(100);
    printf("10, set position to 0, change to speed mode\n");
    memset(dataBuff,0,4);
    format_cmd(0,STP,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    
    //move fast to origin
    int speedHigh = get_init_high_speed(sid);
    if(getELeCtlRetdir(sid) == RET_BACK_POSITIVE)
    {
        speedHigh = 0-speedHigh;
    }
    memcpy(dataBuff,&speedHigh,4);
    usleep(100);
    printf("11, set speed to speedHigh\n");
    format_cmd(0,SPD,sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    // if(sid!=SID_ZR_R)
    // {
    //     dataBuff[0] = 0;
    //     format_cmd(1,MDE,sid,1, dataBuff,&dataRet);
    //     encap_uim_data(&dataRet,&frameToSend);     
    //     this->WriteCanPort(&frameToSend);
    // }
    

}

void CUim_MainCtl::uim_motor_retback(void * paramIn, int sidIn)
{
    int currPos = getELeCtlCurrPos(sidIn);

    if(currPos==0)
    {
        int msg_src = getELeCtlAckFlag(sidIn);
        if(msg_src == MSG_FROM_LUA)
        {
            sendAckToLua(sidIn,CHECK_RET_BACK_END);
        }
        else if(msg_src == MSG_FROM_GRPC)
        {
            sendAckToGrpc(0,0,sidIn,-1);
        }

        return;
    }
    move_param  moveParam;
// #ifdef _QPCR
//    if(sidIn == 45 || sidIn == 46
//     ||sidIn == 52 || sidIn == 53 )
//    {
//         moveParam.dec_speed = 8000;
//         moveParam.speed = 8000;
//         moveParam.inc_speed = moveParam.dec_speed;
//         moveParam.sid = sidIn;
//         uim_ret_back(&moveParam);

//         return;
//    }
// #endif

    int polarity = 1;//the pos should larger than 0;
    
    int retBackPos = get_retback_pos(sidIn);

    uim_data dataRet;
    struct can_frame frameToSend;
    uim_frame dataRead;

    unsigned char dataBuff[4];

    if(getELeCtlRetdir(sidIn) == RET_BACK_NEGITIVE)
    {
       polarity = 0;// the pos should smaller than 0
       if(currPos<0)
       {
           currPos = 0 - currPos;
       }
    }

#define MAX_WAIT_CHECKING_COUNT 80

    move_param * pstMoveParam = (move_param *)paramIn;
    move_param tempParam;
    memcpy(&tempParam,pstMoveParam,sizeof(move_param));
    pstMoveParam = &tempParam;
    int count = 0;
    int sid = pstMoveParam->sid;



    // if(currPos > (retBackPos << 1))
    // {//move fast to the ret back position(away origin short distance) firstly
    //1.1 set speed to 0, switch to position mode 
    memset(dataBuff,0,4);
    format_cmd(0,SPD,pstMoveParam->sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
  
    int acc = get_init_high_acc(pstMoveParam->sid);    
    int speed = get_init_high_speed(pstMoveParam->sid);

    if(currPos <= (retBackPos << 1))
    {
        acc = get_retback_acc(pstMoveParam->sid); 
        speed = get_retback_speed(pstMoveParam->sid);
    }

    int pos = retBackPos;
    //1.2 set the acc,
    
    memcpy(dataBuff,&acc,4);
    format_cmd(0,MAC,pstMoveParam->sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //1.3 set dec,
    int dec = acc;
    memcpy(dataBuff,&dec,4);
    format_cmd(0,MDE,pstMoveParam->sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //1.4 set the pos,
    
    if(polarity==0)
    {
        pos = 0 - pos;
    }
    int tmpPosCode = POS;
    if(getELeCtlLoopType(pstMoveParam->sid) == CLOSE_LOOP)
    {
        tmpPosCode = QEC;
    }

    memcpy(dataBuff,&pos,4);
    format_cmd(0,tmpPosCode,pstMoveParam->sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);
    //1.5 set speed
    //int speed = get_init_high_speed(pstMoveParam->sid);

    memcpy(dataBuff,&speed,4);
    format_cmd(0,SPD,pstMoveParam->sid,4, dataBuff,&dataRet);
    encap_uim_data(&dataRet,&frameToSend);     
    this->WriteCanPort(&frameToSend);

    setELeCtlStatus(pstMoveParam->sid,INIT_AWAY_ORIGING);


    count = 0;
    while(getELeCtlStatus(pstMoveParam->sid) == INIT_AWAY_ORIGING)
    {
        count++;
        usleep(100000);
        if(count>(MAX_WAIT_CHECKING_COUNT<<1))
        {
            break;
        }

        //printf("in loop,count:%d\n",count);
    }

    if(count>(MAX_WAIT_CHECKING_COUNT<<1))
    {
        printf("sid[%d],move to origin nearby timeout,status[%d]\n",pstMoveParam->sid,getELeCtlStatus(sid));
        int msg_src = getELeCtlAckFlag(sid);
        if(msg_src == MSG_FROM_LUA)
        {
            sendAckToLua(sid,CHECK_ACT_TIMEOUT);
        }
        else if(msg_src == MSG_FROM_GRPC)
        {
            sendAckToGrpc(1,ACT_TIMEOUT_APP,sid,-1);
        }
        return;
    }
    //}

    //on origin nearby now
    //and go to origin slow now
    setSensorAction(sid,1);//stop when sensor up or down

    
    memset(&moveParam,0,sizeof(move_param)); 
    moveParam.dec_speed = get_retback_acc(sid);
    moveParam.speed = get_retback_speed(sid);
    moveParam.inc_speed = moveParam.dec_speed;
    moveParam.sid = sid;
    uim_ret_back(&moveParam);
    
    // //1, set the sample mode
    // unsigned char interval[4] = {0x00,0x00,0x00};
    // format_cmd(1,STG,sid,3, interval,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);

    // //4,STOn binding act to S1IE
    // //printf("act ret back now!,sid[%d],222\n",sid);
    // //5.1 set the incease speed
    // int inc_speed = moveParam.inc_speed;
    // printf("ret back, set inc_speed to %d\n",inc_speed);
    // unsigned char incSpeedChar[4] = {inc_speed&0x00ff,
    //             (inc_speed>>8)&0x00ff,(inc_speed>>16)&0x00ff,(inc_speed>>24)&0x00ff 
    // };
    
    // format_cmd(1,MAC,sid,4, incSpeedChar,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);

    // //5.2 set the decrease speed
    // int dec_speed = moveParam.dec_speed;
    // printf("ret back, set dec_speed to %d\n",dec_speed);
    // unsigned char decSpeedChar[4] = {dec_speed&0x00ff,
    //             (dec_speed>>8)&0x00ff,(dec_speed>>16)&0x00ff,(dec_speed>>24)&0x00ff 
    // };
    
    // format_cmd(1,MDE,sid,4, decSpeedChar,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    // //printf("act ret back now!,sid[%d],333\n",sid);

    // memset(dataBuff,0,4);
    // //set relative pos to 0, switch to speed mode
    // format_cmd(1,STP,sid,4, dataBuff,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);
    
    // setELeCtlStatus(sid, RET_BACKING);
    // //5,set the speed
    // speed = moveParam.speed;
    // if(getELeCtlRetdir(sid) == 0)
    // {
    //     speed = 0 - speed;
    // }
    // printf("ret back, set speed to %d\n",speed);
    // unsigned char speedCode[4] = {speed&0x00ff,
    //             (speed>>8)&0x00ff,(speed>>16)&0x00ff,(speed>>24)&0x00ff};
    // format_cmd(1,SPD,sid,4, speedCode,&dataRet);
    // encap_uim_data(&dataRet,&frameToSend);     
    // this->WriteCanPort(&frameToSend);

}